// This is a NEW implementation of the algorithm described in the
// following paper:
//    C. Hertzberg,  R.  Wagner,  U.  Frese,  and  L.  Schroder.  Integratinggeneric   sensor   fusion   algorithms with
//    sound   state   representationsthrough  encapsulation  of  manifolds. CoRR,  vol.  abs/1107.1119,  2011.[Online].
//    Available: http://arxiv.org/abs/1107.1119

/*
 *  Copyright (c) 2019--2023, The University of Hong Kong
 *  All rights reserved.
 *
 *  Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Universitaet Bremen nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

/*
 *  Copyright (c) 2008--2011, Universitaet Bremen
 *  All rights reserved.
 *
 *  Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Universitaet Bremen nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */
/**
 * @file mtk/types/S2.hpp
 * @brief Unit vectors on the sphere, or directions in 3D.
 */
#ifndef S2_H_
#define S2_H_

#include "vect.hpp"

#include "../src/mtkmath.hpp"
#include "SOn.hpp"

namespace MTK {

/**
 * Manifold representation of @f$ S^2 @f$.
 * Used for unit vectors on the sphere or directions in 3D.
 *
 * @todo add conversions from/to polar angles?
 */
template <class _scalar = double, int den = 1, int num = 1, int S2_typ = 3>
struct S2 {
    typedef _scalar scalar;
    typedef vect<3, scalar> vect_type;
    typedef SO3<scalar> SO3_type;
    typedef typename vect_type::base vec3;
    scalar length = scalar(den) / scalar(num);
    enum { DOF = 2, TYP = 1, DIM = 3 };

    // private:
    /**
     * Unit vector on the sphere, or vector pointing in a direction
     */
    vect_type vec;

   public:
    S2() {
        if (S2_typ == 3) vec = length * vec3(0, 0, std::sqrt(1));
        if (S2_typ == 2) vec = length * vec3(0, std::sqrt(1), 0);
        if (S2_typ == 1) vec = length * vec3(std::sqrt(1), 0, 0);
    }
    S2(const scalar &x, const scalar &y, const scalar &z) : vec(vec3(x, y, z)) {
        vec.normalize();
        vec = vec * length;
    }

    S2(const vect_type &_vec) : vec(_vec) {
        vec.normalize();
        vec = vec * length;
    }

    void oplus(MTK::vectview<const scalar, 3> delta, scalar scale = 1) {
        SO3_type res;
        res.w() = MTK::exp<scalar, 3>(res.vec(), delta, scalar(scale / 2));
        vec = res.toRotationMatrix() * vec;
    }

    void boxplus(MTK::vectview<const scalar, 2> delta, scalar scale = 1) {
        Eigen::Matrix<scalar, 3, 2> Bx;
        S2_Bx(Bx);
        vect_type Bu = Bx * delta;
        SO3_type res;
        res.w() = MTK::exp<scalar, 3>(res.vec(), Bu, scalar(scale / 2));
        vec = res.toRotationMatrix() * vec;
    }

    void boxminus(MTK::vectview<scalar, 2> res, const S2<scalar, den, num, S2_typ> &other) const {
        scalar v_sin = (MTK::hat(vec) * other.vec).norm();
        scalar v_cos = vec.transpose() * other.vec;
        scalar theta = std::atan2(v_sin, v_cos);
        if (v_sin < MTK::tolerance<scalar>()) {
            if (std::fabs(theta) > MTK::tolerance<scalar>()) {
                res[0] = 3.1415926;
                res[1] = 0;
            } else {
                res[0] = 0;
                res[1] = 0;
            }
        } else {
            S2<scalar, den, num, S2_typ> other_copy = other;
            Eigen::Matrix<scalar, 3, 2> Bx;
            other_copy.S2_Bx(Bx);
            res = theta / v_sin * Bx.transpose() * MTK::hat(other.vec) * vec;
        }
    }

    void S2_hat(Eigen::Matrix<scalar, 3, 3> &res) {
        Eigen::Matrix<scalar, 3, 3> skew_vec;
        skew_vec << scalar(0), -vec[2], vec[1], vec[2], scalar(0), -vec[0], -vec[1], vec[0], scalar(0);
        res = skew_vec;
    }

    void S2_Bx(Eigen::Matrix<scalar, 3, 2> &res) {
        if (S2_typ == 3) {
            if (vec[2] + length > tolerance<scalar>()) {
                res << length - vec[0] * vec[0] / (length + vec[2]), -vec[0] * vec[1] / (length + vec[2]),
                    -vec[0] * vec[1] / (length + vec[2]), length - vec[1] * vec[1] / (length + vec[2]), -vec[0],
                    -vec[1];
                res /= length;
            } else {
                res = Eigen::Matrix<scalar, 3, 2>::Zero();
                res(1, 1) = -1;
                res(2, 0) = 1;
            }
        } else if (S2_typ == 2) {
            if (vec[1] + length > tolerance<scalar>()) {
                res << length - vec[0] * vec[0] / (length + vec[1]), -vec[0] * vec[2] / (length + vec[1]), -vec[0],
                    -vec[2], -vec[0] * vec[2] / (length + vec[1]), length - vec[2] * vec[2] / (length + vec[1]);
                res /= length;
            } else {
                res = Eigen::Matrix<scalar, 3, 2>::Zero();
                res(1, 1) = -1;
                res(2, 0) = 1;
            }
        } else {
            if (vec[0] + length > tolerance<scalar>()) {
                res << -vec[1], -vec[2], length - vec[1] * vec[1] / (length + vec[0]),
                    -vec[2] * vec[1] / (length + vec[0]), -vec[2] * vec[1] / (length + vec[0]),
                    length - vec[2] * vec[2] / (length + vec[0]);
                res /= length;
            } else {
                res = Eigen::Matrix<scalar, 3, 2>::Zero();
                res(1, 1) = -1;
                res(2, 0) = 1;
            }
        }
    }

    void S2_Nx(Eigen::Matrix<scalar, 2, 3> &res, S2<scalar, den, num, S2_typ> &subtrahend) {
        if ((vec + subtrahend.vec).norm() > tolerance<scalar>()) {
            Eigen::Matrix<scalar, 3, 2> Bx;
            S2_Bx(Bx);
            if ((vec - subtrahend.vec).norm() > tolerance<scalar>()) {
                scalar v_sin = (MTK::hat(vec) * subtrahend.vec).norm();
                scalar v_cos = vec.transpose() * subtrahend.vec;

                res = Bx.transpose() * (std::atan2(v_sin, v_cos) / v_sin * MTK::hat(vec) +
                                        MTK::hat(vec) * subtrahend.vec *
                                            ((-v_cos / v_sin / v_sin / length / length / length / length +
                                              std::atan2(v_sin, v_cos) / v_sin / v_sin / v_sin) *
                                                 subtrahend.vec.transpose() * MTK::hat(vec) * MTK::hat(vec) -
                                             vec.transpose() / length / length / length / length));
            } else {
                res = 1 / length / length * Bx.transpose() * MTK::hat(vec);
            }
        } else {
            std::cerr << "No N(x, y) for x=-y" << std::endl;
            std::exit(100);
        }
    }

    void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res) {
        Eigen::Matrix<scalar, 3, 2> Bx;
        S2_Bx(Bx);
        res = 1 / length / length * Bx.transpose() * MTK::hat(vec);
    }

    void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta) {
        Eigen::Matrix<scalar, 3, 2> Bx;
        S2_Bx(Bx);
        if (delta.norm() < tolerance<scalar>()) {
            res = -MTK::hat(vec) * Bx;
        } else {
            vect_type Bu = Bx * delta;
            SO3_type exp_delta;
            exp_delta.w() = MTK::exp<scalar, 3>(exp_delta.vec(), Bu, scalar(1 / 2));
            res = -exp_delta.toRotationMatrix() * MTK::hat(vec) * MTK::A_matrix(Bu).transpose() * Bx;
        }
    }

    operator const vect_type &() const { return vec; }

    const vect_type &get_vect() const { return vec; }

    friend S2<scalar, den, num, S2_typ> operator*(const SO3<scalar> &rot, const S2<scalar, den, num, S2_typ> &dir) {
        S2<scalar, den, num, S2_typ> ret;
        ret.vec = rot * dir.vec;
        return ret;
    }

    scalar operator[](int idx) const { return vec[idx]; }

    friend std::ostream &operator<<(std::ostream &os, const S2<scalar, den, num, S2_typ> &vec) {
        return os << vec.vec.transpose() << " ";
    }
    friend std::istream &operator>>(std::istream &is, S2<scalar, den, num, S2_typ> &vec) {
        for (int i = 0; i < 3; ++i) is >> vec.vec[i];
        vec.vec.normalize();
        vec.vec = vec.vec * vec.length;
        return is;
    }
};

}  // namespace MTK

#endif /*S2_H_*/
